#include "sim_obstacleLaser.h"


ObstacleLaser::ObstacleLaser(ros::NodeHandle& nh, std::string publish_frame)
{
    mNode = nh;
    pub_frame = publish_frame;
    scan_pub = mNode.advertise<sensor_msgs::LaserScan>("/virtual_scan", 50);
    newObstacle.resize(3);

    num_readings =360;
    ranges.resize(num_readings);
    intensities.resize(num_readings);
    scan.header.frame_id = pub_frame;
    scan.angle_min = -Pii;
    scan.angle_max = Pii;
    scan.angle_increment = 2*Pii / num_readings;
    scan.time_increment = 0;
    scan.range_min = 0.0;
    scan.range_max = 100.0;
    scan.ranges.resize(num_readings);
    scan.intensities.resize(num_readings);

    //Add walls and boxes.
    //The box.
    addWallObstacle(0.87, 0.45, 0.05);
    addWallObstacle(0.87, 0.76, 0.05);
    addWallObstacle(1.3, 0.45, 0.05);
    addWallObstacle(1.3, 0.76, 0.05);

    //Baskets.
    addWallObstacle(0.2, -1.7, 0.17);
    addWallObstacle(3.5, -0.8, 0.17);

    //Simulation testing.
    //addWallObstacle(2,-2,0.5);


    //Add all wall obstacles to the obstaclelist.
    addWallObstacles();
}


double ObstacleLaser::limitAngle(double prevAngle)
{
	//Limit angle between 0-2Pi.
	return prevAngle-floor(prevAngle/(2*Pii))*2*Pii;
}

double ObstacleLaser::mes_index_to_angle(int mes_num)
{
	//Convert measurement index to angle between 0...2Pi
	return limitAngle( (mes_num-180) * Pii / 180);
}

int ObstacleLaser::angle_to_index(double angle)
{
	//Convert angle to mes index 0..360
	int ind1;
	ind1 = round(limitAngle(angle)*180/Pii);
	if(ind1 > 179) return ind1-180;
	else return ind1+180;
}

double ObstacleLaser::angle_difference(double angle1, double angle2)
{
	double diff = limitAngle(angle1 - angle2);
	if(diff > Pii) diff = 2*Pii - diff;
	return diff;
}

void ObstacleLaser::addObstacle(double x, double y, double r)
{
    newObstacle[0] = x;
    newObstacle[1] = y;
    newObstacle[2] = r;
    obstaclePoints.push_back(newObstacle);
}

void ObstacleLaser::addWallObstacle(double x, double y, double r)
{
    newObstacle[0] = x;
    newObstacle[1] = y;
    newObstacle[2] = r;
    wallObstaclePoints.push_back(newObstacle);
}

void ObstacleLaser::generateLaserScan(const sensor_msgs::LaserScan::ConstPtr& real_scan)
{
    mListener.waitForTransform("/map", pub_frame, ros::Time(0), ros::Duration(1));
    
    //Get laser's current pose.
    mListener.lookupTransform("/map",pub_frame,ros::Time(0), currentTransform);
    LaserAngle = limitAngle(tf::getYaw(currentTransform.getRotation()));
    LaserX = currentTransform.getOrigin().x();
    LaserY = currentTransform.getOrigin().y();

    //First the laser sees nothing.
    for(int i = 0; i < num_readings; i++)
    {
        intensities[i] = 0.0;
        ranges[i] = 100.0;
    }

    //Add obstacles to the scan one by one.
    for(int i = 0; i < obstaclePoints.size(); i++)
    {
        //Calculate measurement range info for obstacle.
        OMA = limitAngle(atan2(obstaclePoints[i][1]-LaserY, obstaclePoints[i][0]-LaserX));
        OLA = limitAngle(OMA-LaserAngle);
        OLD = sqrt(pow(obstaclePoints[i][1]-LaserY,2)+pow(obstaclePoints[i][0]-LaserX,2));
        OLAdiff = atan(obstaclePoints[i][2]/OLD);

        //Check that object is not too close.
        if((OLD > 0.6) || ((OLA > 0.26) && (OLA < 2*Pii-0.26)))
        {
            //Range as measurement index.
            OLIstart = angle_to_index(OLA - OLAdiff);
            OLIend = angle_to_index(OLA + OLAdiff);
            if(OLIend < 0 || OLIend > 359) std::cout << "OLIend not in range!"<< std::endl;

            //Set individual measurements.
            int k=0;
            for(int j=OLIstart; j < 1000; j++)
            {
                if(j > 359) j = j - 360;

                //Calculate obstacle istance in the direction.
                alpha = std::abs(k*Pii/180 - OLAdiff);
                if(OLD > 0.5) rsin = (obstaclePoints[i][2] + 0.0) / sin(alpha);
                else rsin = obstaclePoints[i][2] / sin(alpha);
                gamma = Pii - asin(OLD/rsin);
                theta = Pii - alpha - gamma;
                point_distance = rsin*sin(theta);

                //Test if obstacle is seen.
                if(ranges[j] > point_distance) //(OLD-obstaclePoints[i][2]))
                {
                    //Add obstacle to the measurement.
                    intensities[j] = 1.0;
                    //ranges[j] = OLD - obstaclePoints[i][2];
                    ranges[j] = point_distance;
                }
                if(j == OLIend) break;
                if((j > 359) || (j < 0)) std::cout << "j out of range!" << std::endl;
                k++;
            }
        }
    }

    //Create and publish the generated sensor message.
    scan_time = ros::Time::now();
    scan.header.stamp = scan_time;
    
    //Determine which is seen, virtual obstacles or real scan.
    for(unsigned int i = 0; i < num_readings; ++i)
    {
        //Real system:
        /*if((i >= 45) && (i <= 270))
        {
            scan.ranges[i]= std::min(std::min(real_scan->ranges[(i-45)*2+1],real_scan->ranges[(i-45)*2]), float(ranges[i]));
            scan.intensities[i] = std::max(std::max(real_scan->intensities[(i-45)*2+1],real_scan->intensities[(i-45)*2]), float(intensities[i]));
        }
        else
        {
            scan.ranges[i] = ranges[i];
            scan.intensities[i] = intensities[i];
        }*/

        //Simulation:
        if((i >= 90) && (i < 270))
        {
            scan.ranges[i]= std::min(std::min(real_scan->ranges[(i-90)*2+1],real_scan->ranges[(i-90)*2]), float(ranges[i]));
            scan.intensities[i] = std::max(std::max(real_scan->intensities[(i-90)*2+1],real_scan->intensities[(i-90)*2]), float(intensities[i]));
        }
        else
        {
            scan.ranges[i] = ranges[i];
            scan.intensities[i] = intensities[i];
        }
    }
    scan_pub.publish(scan);
}


void ObstacleLaser::updateObstacle(const group6::DetectedObject::ConstPtr& objects)
{
    addObstacle(objects->x, objects->y, 0.5);
}

void ObstacleLaser::updateObstacles(const group6::DetectedObjectArray::ConstPtr& objects)
{
    std::cout << "Setting " << objects->size << " obstacles." << std::endl;
    obstaclePoints.clear();
    for(int i=0; i < objects->size; i++)
    {
        addObstacle(objects->objects[i].x, objects->objects[i].y, 0.05);
    }

    addWallObstacles();
    std::cout << "Obstacles ready." << std::endl;
}

void ObstacleLaser::addWallObstacles()
{
    std::cout << "Setting " << wallObstaclePoints.size() << " wall obstacles." << std::endl;
    for(int i = 0; i < wallObstaclePoints.size(); i++)
    {
        obstaclePoints.push_back(wallObstaclePoints[i]);

    }
}

int main(int argc, char** argv){
//Basic params.
ros::init(argc, argv, "virtual_laser");
ros::NodeHandle n;

//Create obstaclelaser instance.
ObstacleLaser obslaser(n, "base_laser_link");

//Generate virtual scan always when real one is published.
ros::Subscriber mLaserScanSubscriber = n.subscribe("base_scan", 1, &ObstacleLaser::generateLaserScan, &obslaser);

//Update obstacles.
//ros::Subscriber mObstacleSubscriber = n.subscribe("obstacles", 1, &ObstacleLaser::updateObstacle, &obslaser);
ros::Subscriber mObstacleSubscriber = n.subscribe("object_list", 10, &ObstacleLaser::updateObstacles, &obslaser);

ros::spin();
std::cout << "Out of loop!" << std::endl;
}
